#include <boost/progress.hpp>
#include <boost/timer.hpp>
#include <cfloat>
#include <eigen3/Eigen/Dense>
#include <fstream>
#include <functional> // std::bind
#include <iostream>
#include <iostream> // std::cout
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <vector>
#include <unordered_map>

// #include <opencv2/opencv.hpp>
#define M_PI 3.14159265358979323846
#define M_2PI 6.28318530717958647692
template <typename T>
void DumpMsg(int64_t time_stamp, T const &msg, int size,
             const std::string &msg_name) {
  std::string file_name = std::to_string(time_stamp) + "_" + msg_name + ".bin";
  std::ofstream ofs(file_name, std::ofstream::binary);
  ofs.write(reinterpret_cast<char *>(msg), size);
  ofs.close();
  std::cout << "ok";
}
float PythonMod(float x, float y) {
  return std::fmod((std::fmod(x, y) + y), y);
}

void Unwrap(const std::vector<float> &yaw_array,
            std::vector<float> &unwrap_yaw_array) {
  unwrap_yaw_array.clear();
  float cumsum_ph = 0;
  unwrap_yaw_array.push_back(yaw_array[0]);
  for (size_t n = 1; n < yaw_array.size(); n++) {
    float diff = yaw_array[n] - yaw_array[n - 1];
    float diff_mod = PythonMod((diff + M_PI), (2 * M_PI)) - M_PI;
    if (diff_mod == -M_PI && diff > 0) {
      diff_mod = M_PI;
    }
    float ph_correct = diff_mod - diff;
    if (std::fabs(diff) < M_PI) {
      ph_correct = 0;
    }
    cumsum_ph += ph_correct;
    float up = yaw_array[n] + cumsum_ph;
    up = PythonMod(up, (2 * M_PI));
    unwrap_yaw_array.push_back(up);
  }
}

struct Lanes {
  int id{0};
};

template <class T> struct Point {
  T x, y, z, yaw, s;

  Point() : x(0), y(0), z(0), yaw(0), s(0) {}

  Point(T x, T y, T z, T yaw, T s) : x(x), y(y), z(z), yaw(yaw), s(s) {}

  Point(T x, T y) : x(x), y(y), z(0), yaw(0), s(0) {}
};

template <typename T> T StdClamp(T _input, T _min, T _max) {
  if (_min > _max) {
    std::swap(_min, _max);
  }
  T res = _input;
  res = std::min(std::max(res, _min), _max);
  return res;
}

template <typename T>
void TranslationAndRotation(const T x0, const T y0, const T theta, const T dx,
                            const T dy, T &x, T &y) {
  const T cos_theta = std::cos(theta);
  const T sin_theta = std::sin(theta);

  x = cos_theta * dx - sin_theta * dy + x0;
  y = sin_theta * dx + cos_theta * dy + y0;
}

class TrackingDifferentiator {
public:
  double r_ = 1.0; // max input
  double h_ = 0.1; // sample time

  double x1_ = 0.0;
  double x2_ = 0.0;

public:
  void Init(const double r, const double h, const double x1) {
    r_ = r;
    h_ = h;
    x1_ = x1;
    x2_ = 0.0;
  }

  double Apply(const double v) {
    double fh = Fhan(x1_ - v, x2_);
    x1_ += h_ * x2_;
    x2_ += h_ * fh;
    return x2_;
  }

  double Fhan(const double x1, const double x2) {
    double d = r_ * h_ * h_;
    double a0 = h_ * x2;
    double y = x1 + a0;
    double a1 = std::sqrt(d * (d + 8.0 * std::fabs(y)));
    double a2 = a0 + Sign(y) * (a1 - d) * 0.5;
    double fsgy = Fsg(y, d);
    double a = (a0 + y) * fsgy + a2 * (1.0 - fsgy);
    double fsga = Fsg(a, d);
    double fhan = -r_ * (a / d) * fsga - r_ * Sign(a) * (1.0 - fsga);
    return fhan;
  }

  double Fsg(const double x, const double d) {
    return (Sign(x + d) - Sign(x - d)) * 0.5;
  }

  double Sign(const double x) { return std::copysign(1.0, x); }
};

struct Dignose {
  int a = 0;
  std::vector<int> b;
  void SetA(int a_tmp) { a = a_tmp; }
};

void ChangeValue(const Dignose &dig, int &a) { a = 10; }

// 计算射线延长指定距离的点
inline std::vector<float> ExtendNewPoint(float xa, float xb, float ya, float yb,
                                         float d) {
  float xab, yab, xbd, ybd;
  float xd, yd;
  // xab = xb - xa;
  // yab = yb - ya;
  // ybd = sqrt(powf(d, 2) / (powf(xab / yab, 2) + 1));
  // ybd = yab > 0.0f ? ybd : -ybd;
  // yd = ybd + yb;
  // xd = ybd * xab / yab + xb;
  float theta = atan2(yb - ya, xb - xa);
  xd = xb + d * cos(theta);
  yd = yb + d * sin(theta);
  xd = abs(xd) < 1e-5 ? 0.0f : xd;
  yd = abs(yd) < 1e-5 ? 0.0f : yd;
  return {xd, yd};
}

// 计算直线的交点
inline bool CrossPoint(float xa, float xb, float xc, float xd, float ya,
                       float yb, float yc, float yd, float &x, float &y) {
  float a1 = yb - ya;
  float b1 = xa - xb;
  float c1 = xb * ya - xa * yb;
  float a2 = yd - yc;
  float b2 = xc - xd;
  float c2 = xd * yc - xc * yd;
  float denominator = a1 * b2 - a2 * b1;
  if (std::fabs(denominator) < 0.01)
    return false;
  y = (c1 * a2 - c2 * a1) / denominator;
  x = (c2 * b1 - c1 * b2) / denominator;
  return true;
}

void ConstReferenceToReference(const Dignose &dig, std::vector<int> &out_b) {
  const auto &b = dig.b;
  out_b = b;
}

template <typename T> struct LinePoint {
  T x, y, z, yaw, k;
  int64_t time;
};

inline bool RapidRejExper(const LinePoint<float> &p1,
                          const LinePoint<float> &p2,
                          const LinePoint<float> &p3,
                          const LinePoint<float> &p4) {
  if (std::min(p1.x, p2.x) > std::max(p3.x, p4.x) ||
      std::min(p1.y, p2.y) > std::max(p3.y, p4.y) ||
      std::min(p3.x, p4.x) > std::max(p1.x, p2.x) ||
      std::min(p3.y, p4.y) > std::max(p1.y, p2.y)) {
    return false;
  }
  return true;
}

inline bool Cross(const LinePoint<float> &p1, const LinePoint<float> &p2,
                  const LinePoint<float> &p3, const LinePoint<float> &p4) {
  float v1_x = p3.x - p1.x;
  float v1_y = p3.y - p1.y;
  float v2_x = p2.x - p1.x;
  float v2_y = p2.x - p1.x;
  float v3_x = p4.x - p1.x;
  float v3_y = p4.y - p1.y;
  return (v1_x * v2_y - v2_x * v1_y) * (v3_x * v2_y - v2_x * v3_y) <= 0;
}

inline float CosTheta(const LinePoint<float> &p1, const LinePoint<float> &p2,
                      const LinePoint<float> &p3, const LinePoint<float> &p4) {
  float v1_x = p2.x - p1.x;
  float v1_y = p2.y - p1.y;
  float v2_x = p4.x - p3.x;
  float v2_y = p4.y - p3.y;
  return (v1_x * v2_x + v1_y * v2_y) /
         (sqrt(v1_x * v1_x + v1_y * v1_y) * sqrt(v2_x * v2_x + v2_y * v2_y));
}

template <typename T>
void GetIntervalIndexByT(const std::vector<T> &points, const int64_t t,
                         int *const i_1st, int *const i_2nd,
                         float *const weight = NULL,
                         int *const i_start = NULL) {
  if (points.size() < 2)
    return;
  if (!i_start) {
    *i_2nd = std::upper_bound(points.begin(), points.end(), t,
                              [](const int64_t t, const T &point) -> bool {
                                return t < point.time;
                              }) -
             points.begin();
    if (*i_2nd <= 0) {
      ++*i_2nd;
    } else if (*i_2nd >= points.size()) {
      --*i_2nd;
    }
    *i_1st = *i_2nd - 1;
  } else if (*i_start + 1 >= points.size()) {
    *i_start = *i_1st = points.size() - 2;
    *i_2nd = *i_1st + 1;
  } else {
    *i_2nd = *i_start + 1;
    while (*i_2nd < (points.size() - 1) && points.at(*i_2nd).time <= t) {
      ++*i_2nd;
    }
    *i_start = *i_1st = *i_2nd - 1;
  }
  if (weight) {
    *weight =
        (static_cast<float>(t - points.at(*i_1st).time)) /
        (static_cast<float>(points.at(*i_2nd).time - points.at(*i_1st).time));
  }
}

/**
 * @brief Predict Kinematic by CTRV (Constant Turn Rate and Velocity)
 * @param x0 init pose x
 * @param y0 init pose y
 * @param yaw0 init pose yaw
 * @param v0 init velocity
 * @param yaw_rate0 init yaw_rate
 * @param t predict time
 * @param x1 final pose x
 * @param y1 final pose y
 * @param yaw1 final pose yaw
 * @param v1 final pose speed
 * @param yaw_rate1 final yaw_rate
 * @return final pose
 */
template <typename T>
void KinematicModelPrediction(const T x0, const T y0, const T yaw0, const T v0,
                              const T yaw_rate0, const T t, T &x1, T &y1,
                              T &yaw1, T &v1, T &yaw_rate1) {
  v1 = v0;
  yaw_rate1 = yaw_rate0;
  yaw1 = yaw_rate0 * t + yaw0;
  T sin_yaw0 = std::sin(yaw0);
  T cos_yaw0 = std::cos(yaw0);
  T sin_yaw1 = std::sin(yaw1);
  T cos_yaw1 = std::cos(yaw1);
  if (yaw_rate0 == 0) {
    x1 = v0 * cos_yaw0 * t + x0;
    y1 = v0 * sin_yaw0 * t + y0;
  } else {
    x1 = v0 / yaw_rate1 * sin_yaw1 - v0 / yaw_rate0 * sin_yaw0 + x0;
    y1 = -v0 / yaw_rate1 * cos_yaw1 + v0 / yaw_rate0 * cos_yaw0 + y0;
  }
}

/**
 * @brief Predict Kinematic by CTRA (Constant Turn Rate and Acceleration)
 * @param x0 init pose x
 * @param y0 init pose y
 * @param yaw0 init pose yaw
 * @param v0 init velocity
 * @param yaw_rate0 init yaw_rate
 * @param t predict time
 * @param x1 final pose x
 * @param y1 final pose y
 * @param yaw1 final pose yaw
 * @param v1 final pose speed
 * @param yaw_rate1 final yaw_rate
 * @return final pose
 */
template <typename T>
void KinematicModelPrediction(const T x0, const T y0, const T yaw0, const T v0,
                              const T a0, const T yaw_rate0, const T t, T &x1,
                              T &y1, T &yaw1, T &v1, T &yaw_rate1, T &a1) {
  v1 = v0;
  yaw_rate1 = yaw_rate0;
  yaw1 = yaw_rate0 * t + yaw0;
  T sin_yaw0 = std::sin(yaw0);
  T cos_yaw0 = std::cos(yaw0);
  T sin_yaw1 = std::sin(yaw1);
  T cos_yaw1 = std::cos(yaw1);
  x1 = 1 / ((yaw_rate0 * yaw_rate0) *
                ((v0 * yaw_rate0 + a0 * yaw_rate0 * t) * sin_yaw1 +
                 yaw_rate0 * t) +
            a0 * cos_yaw1) -
       v0 * yaw_rate0 * sin_yaw0 - a0 * cos_yaw0 + x0;
  y1 = 1 / ((yaw_rate0 * yaw_rate0) *
                ((-v0 * yaw_rate0 + a0 * yaw_rate0 * t) * cos_yaw1 +
                 yaw_rate0 * t) +
            a0 * sin_yaw1) -
       v0 * yaw_rate0 * cos_yaw0 - a0 * sin_yaw0 + y0;

}

/**
 * @brief Unify theta between -PI and PI.
 * @param theta The origin value.
 * @return Unified angle.
 */
template <typename T>
T UnifyTheta(T theta) {
  while (theta > M_PI) {
    theta -= M_2PI;
  }
  while (theta < -M_PI) {
    theta += M_2PI;
  }
  return theta;
}

/**
 * @brief Calculate the weighted average value of two angles. The result is
 * unified between -PI and PI.
 * @param t1 The first angle.
 * @param t2 The second angle.
 * @param weight The value of (result - t1)/(t2 - t1).
 */
template <typename T>
inline T LinearInterpolationTheta(T t1, T t2, float weight) {
  if (t1 > t2) {
    std::swap(t1, t2);
    weight = 1 - weight;
  }
  while (std::abs(t2 - t1) > M_PI && t2 > t1) {
    t2 -= M_2PI;
  }
  return UnifyTheta((1.0f - weight) * t1 + weight * t2);
}

struct Object {
  std::string id;
  bool lat_valid;
  int main_bind_lane;
};

struct PredictionInfo {
  explicit PredictionInfo(const Object &object) {
    id = object.id;
    // his_traj.valid = false;
    lat_valid = true;
    // pred_trajs.clear();
    // his_traj.trajectory.clear();
    lat_pred_prob.clear();
    main_bind_lane = object.main_bind_lane;
    pred_bind_lane = object.main_bind_lane;
     std::cout << "显示构造函数被调用" << std::endl;
  }
  PredictionInfo() {
    std::cout << "默认构造函数被调用" << std::endl;
  };
  std::string id;
  bool lat_valid;
  int main_bind_lane;
  int pred_bind_lane;
  // HistoryInfo his_traj;
  // std::vector<PredictTraj> pred_trajs;
  std::vector<float> lat_pred_prob;
};
